#ifdef SYPX_MEMCHECK
#include "mmgr.h"
#endif

#include <math.h>
#include "Instance.h"
#include "Instances.h"
#include "Actor.h"
#include "Actors.h"
#include "Level.h"
#include <sstream>

namespace SheepDefence 
{
	Instance::Instance()
	{
	}

	Instance::~Instance()
	{
		delete sprite;
	}
	
	void Instance::move(int x, int y)
	{
	}

	void Instance::changeState(instanceState new_state)
	{
		if (state != new_state)
		{
			state = new_state;
			changed = true;
			stateAge = 0;
		}
	}

	bool Instance::stateChanged()
	{
		bool result = changed;
		changed = false;
		return result;
	}

	void Instance::act(float time)
	{
	}

	void Instance::respond()
	{
	}

	void Instance::boundaryCheck(Actor* actor)
	{
		dest_x = dest_x > 310 - actor->collision / 2 ? 310 - actor->collision / 2 : dest_x < actor->collision / 2 ? actor->collision / 2 : dest_x;
		dest_y = dest_y > 470 - actor->collision / 2 ? 470 - actor->collision / 2 : dest_y < actor->collision / 2 ? actor->collision / 2 : dest_y;
	}

	bool Instance::collide(Instance* target)
	{
		return false;
	}

	void Instance::updateAttributes(SypX::Vec2 movement)
	{
	}

	void Instance::updateLevel(Level* lvl)
	{
		level = lvl;
	}

	void Instance::advance(float time)
	{
		if (state == UNSPAWNED || state == DEAD || state == BITTEN || state == BITING)
			return;

		boundaryCheck(getActor());
		
		SypX::Vec2 direction;
		float dist = (dest_x - x) * (dest_x - x) + (dest_y - y) * (dest_y - y);

		if (dist == 0)
			return;

		float ratio = (getActor()->speed * time) / sqrt(dist);

		if (state == POUNCING)
			ratio *= 4;
		else if (stamState == TIRED)
			ratio *= 0.5;

		if (ratio > 1)
			ratio = 1;

		direction.x = ratio * (dest_x - x);
		direction.y = ratio * (dest_y - y);

		// Collision detection
		float new_x = x + direction.x;
		float new_y = y + direction.y;
		int attempts = 0;							// attempts to avoid collision
		void (*turnFunction) (SypX::Vec2&) = NULL;	// maintain direction

		std::vector<Instance*>::iterator i = level->instances.begin();
		
		while (i != level->instances.end() && state != POUNCING && state != BITING)	// pounce across obstacles ok, collision checked prior to pounce
		{
			if ((*i)->type == OBSTACLE || !collide(*i))
			{
				i++;
				continue;
			}

			float distance = getDist((*i)->x - new_x, (*i)->y - new_y);

			float limit = (float) pow((float) ((*i)->getActor()->collision + getActor()->collision), 2);

			if (distance < limit - attempts)	// protection against infinite loop in rare case
			{
				if (attempts == 0)
				{
					SypX::Vec2 checker;
					checker.x = (*i)->x - x;
					checker.y = (*i)->y - y;

					float obstruction_angle = getAngle(checker);
					float facing_angle = getAngle(direction);

					float right = obstruction_angle - facing_angle + 12;
					right = right >= 12 ? right - 12 : right;

					float left = facing_angle - obstruction_angle + 12;
					left = left >= 12 ? left - 12 : left;

					if (right >= left)
						turnFunction = turnRight;
					else
						turnFunction = turnLeft;
				}

				turnFunction(direction);

				new_x = x + direction.x;
				new_y = y + direction.y;

				attempts++;
				i = level->instances.begin();
				continue;
			}
			
			i++;
		}

		std::vector<ObstacleInstance*>::iterator j = level->obstacles.begin();

		while (j != level->obstacles.end() && (getActor()->type == SHEEP || state == FLEEING || pathState == BANGING))	// pounce across obstacles ok, collision checked prior to pounce
		{
			std::vector<circle*>::iterator k = (*j)->actor->circles.begin();

			while (k != (*j)->actor->circles.end())
			{
				float distance = getDist((*j)->x + (*j)->actor->center_x + (*k)->x[(*j)->angle] - new_x, (*j)->y + (*j)->actor->center_y + (*k)->y[(*j)->angle] - new_y);
				float limit = (float) pow((float) ((*k)->r + getActor()->collision), 2);

				if (distance < limit)
				{
					changeState(STUCK);
					return;
				}

				k++;
			}

			std::vector<line*>::iterator m = (*j)->actor->lines.begin();

			while (m != (*j)->actor->lines.end())
			{
				float distance = getDist(new_x, new_y, (*j)->x + (*j)->actor->center_x + (*m)->x1[(*j)->angle], (*j)->y + (*j)->actor->center_y + (*m)->y1[(*j)->angle], (*j)->x + (*j)->actor->center_x + (*m)->x2[(*j)->angle], (*j)->y + (*j)->actor->center_y + (*m)->y2[(*j)->angle]);
				float limit = (float) getActor()->collision * getActor()->collision;

				if (distance < limit)
				{
					if (getActor()->type == WOLF)
						SypX::ErrorLogger::getLogger() << "WOLF!!\n";

					changeState(STUCK);
					return;
				}

				m++;
			}

			j++;
		}

		updateAttributes(direction);

		SypX::ErrorLogger::getLogger() << level->timer_25 << "\n";

		if (level->timer_25 == 0 || stateChanged())
		{
			static double times = 0;
			times++;
			SypX::ErrorLogger::getLogger() << times << "\n";
			SypX::Vec2 aggregate_dir;
			aggregate_dir.x = new_x - prev_x;
			aggregate_dir.y = new_y - prev_y;

			if (getDist(aggregate_dir.x, aggregate_dir.y) > 10)
			{
				angle = genAngle(aggregate_dir);
				sprite->setActiveAngle(angle);
				prev_x = new_x;
				prev_y = new_y;
			}
		}
		const SypX::Vec4& spPos = sprite->getPositionRef();

		float z = 0.75f - (spPos.y + direction.y - getActor()->collision / 2) / 960.0f;

		if (state == BITTEN || state == DEAD)	// TODO: move to act
			z -= 0.25;

		if (state == BITING || state == POUNCING)
			z += 0.25;

		sprite->setPosition(SypX::Vec3(spPos.x + direction.x, spPos.y + direction.y, z));
		
		x = spPos.x;
		y = spPos.y;

		if (state == STUCK)
			changeState(FLEEING);
	}

	void Instance::draw()
	{
		if (getActor()->type == DOG)
		{
			int k = static_cast<DogInstance*>(this)->exceptions.size();
			std::ostringstream oss;
			oss << k;
			level->font->drawText(oss.str(), x, y + 10, 1.0f);
		}
		else if (state == FLEEING)
			level->font->drawText("!", x, y + 10, 1.0f);
		else if (state == BANGING)
			level->font->drawText("?", x, y + 10, 1.0f);
		
		sprite->update();
	}

	float Instance::getAngle(SypX::Vec2& direction)
	{
		float x = direction.x, y = direction. y, t = y / x;
		float r;

		if (x < 0)
			r = (float) (3 - atan(t) * 1.91);
		else if (x == 0 && y > 0)
			r = 6;
		else if (x == 0)
			r = 0;
		else
			r = (float) (9 - atan(t) * 1.91);

		return r >= 12 ? r - 12 : (r < 0 ? r + 12: r);
	}

	int Instance::genAngle(SypX::Vec2& direction)
	{
		return (int) floor(getAngle(direction) + 0.5) % 12;
	}

	float Instance::getDist(Instance* a, Instance* b)
	{
		return (a->x - b->x) * (a->x - b->x) + (a->y - b->y) * (a->y - b->y);
	}

	float Instance::getDist(float diff_x, float diff_y)
	{
		return diff_x * diff_x + diff_y * diff_y;
	}

	float Instance::getDist(float x, float y, float a_x, float a_y, float b_x, float b_y)
	{
		float a = x - a_x;
		float b = y - a_y;
		float c = b_x - a_x;
		float d = b_y - a_y;

		float dot_product = a * c + b * d;
		float length_sqr = c * c + d * d;
		float pnt_param = dot_product / length_sqr;

		float p_x, p_y;		// point on line closest to obstacle;

		if (pnt_param < 0) 
		{
			p_x = a_x;
			p_y = a_y;
		}
		else if (pnt_param > 1)
		{
			p_x = b_x;
			p_y = b_y;
		}
		else
		{
			p_x = a_x + pnt_param * c;
			p_y = a_y + pnt_param * d;
		}

		return getDist(x - p_x, y - p_y);
	}

	void Instance::rotateByAngle(SypX::Vec2& direction, float angle)
	{
		float t = direction.x;

		direction.x = cos(angle) * direction.x - sin(angle) * direction.y;
		direction.y = sin(angle) * t + cos(angle) * direction.y;
	}

	void Instance::turnRight(SypX::Vec2& direction)
	{
		rotateByAngle(direction, 5.78f);
	}

	void Instance::turnLeft(SypX::Vec2& direction)
	{
		rotateByAngle(direction, 0.5f);
	}

	// line collision with obstacles without exceptions
	bool Instance::collision(float start_x, float start_y, float end_x, float end_y, std::vector<ObstacleInstance*>& obstacles, float size)
	{
		std::vector<ObstacleInstance*> blank;

		return collision(start_x, start_y, end_x, end_y, obstacles, size, blank);
	}

	// line collision with obstacles
	bool Instance::collision(float start_x, float start_y, float end_x, float end_y, std::vector<ObstacleInstance*>& obstacles, float size, std::vector<ObstacleInstance*>& exceptions)
	{
		std::vector<ObstacleInstance*>::iterator i = obstacles.begin();

		bool cont;

		while (i != obstacles.end())
		{
			cont = false;

			if (exceptions.size() > 0)
			{
				std::vector<ObstacleInstance*>::iterator k = exceptions.begin();

				while (k != exceptions.end())
				{
					if ((*i) == (*k))
					{
						cont = true;
						break;
					}

					k++;
				}

				if (cont)
				{
					i++;
					continue;
				}
			}

			std::vector<line*>::iterator h = (*i)->actor->lines.begin();

			while (h != (*i)->actor->lines.end())
			{
				float x1 = (*i)->x + (*i)->actor->center_x + (*h)->x1[(*i)->angle];
				float y1 = (*i)->y + (*i)->actor->center_y + (*h)->y1[(*i)->angle];
				float x2 = (*i)->x + (*i)->actor->center_x + (*h)->x2[(*i)->angle];
				float y2 = (*i)->y + (*i)->actor->center_y + (*h)->y2[(*i)->angle];

				float u_x = end_x - start_x;
				float u_y = end_y - start_y;

				float v_x = x2 - x1;
				float v_y = y2 - y1;

				float w_x = start_x - x1;
				float w_y = start_y - y1;

				float a = u_x * u_x + u_y * u_y;
				float b = u_x * v_x + u_y * v_y;
				float c = v_x * v_x + v_y * v_y;
				float d = u_x * w_x + u_y * w_y;
				float e = v_x * w_x + v_y * w_y;

				float D = a * c - b * b;

				float sc, sN, sD = D;
				float tc, tN, tD = D;

				if (D < 0.1)
				{
					sN = 0;
					sD = 1;
					tN = e;
					tD = c;
				}
				else 
				{
					sN = b * e - c * d;
					tN = a * e - b * d;

					if (sN < 0)
					{
						sN = 0;
						tN = e;
						tD = c;
					}
					else if (sN > sD)
					{
						sN = sD;
						tN = e + b;
						tD = c;
					}
				}

				if (tN < 0)
				{
					tN = 0;

					if (-d < 0)
						sN = 0;
					else if (-d > a)
						sN = sD;
					else
					{
						sN = -d;
						sD = a;
					}
				}
				else if (tN > tD)
				{
					tN = tD;

					if ((b - d) < 0)
						sN = 0;
					else if ((b - d) > a)
						sN = sD;
					else
					{
						sN = b - d;
						sD = a;
					}
				}

				sc = abs(sN) < 0.1 ? 0 : sN / sD;
				tc = abs(tN) < 0.1 ? 0 : tN / tD;

				float d_x = w_x + sc * u_x - tc * v_x;
				float d_y = w_y + sc * u_y - tc * v_y;

				if (getDist(d_x, d_y) < (size * size))
					return true;

				h++;
			}

			std::vector<circle*>::iterator j = (*i)->actor->circles.begin();

			while(j != (*i)->actor->circles.end())
			{
				if (getDist((*i)->x + (*i)->actor->center_x + (*j)->x[(*i)->angle], (*i)->y + (*i)->actor->center_y + (*j)->y[(*i)->angle], start_x, start_y, end_x, end_y) < pow((float)((*j)->r + size), 2))
					return true;
				j++;
			}

			i++;
		}

		return false;
	}

	bool Instance::collision(float start_x, float start_y, float end_x, float end_y, std::vector<ObstacleInstance*>& obstacles, WolfInstance* wolf, DogInstance* dog)
	{
		if (collision(start_x, start_y, end_x, end_y, obstacles, wolf->actor->collision))
			return true;
		else 
			return (getDist(dog->x, dog->y, start_x, start_y, end_x, end_y) < pow((float)(dog->actor->fear + wolf->actor->fear), 2));	// note, checking against dog's fear
	}

	std::vector<ObstacleInstance*> Instance::inObstacle(float x, float y, std::vector<ObstacleInstance*>& obstacles, float size)
	{
		std::vector<ObstacleInstance*> result;

		std::vector<ObstacleInstance*>::iterator i = obstacles.begin();

		bool cont;

		while(i != obstacles.end())
		{
			cont = false;
			
			std::vector<line*>::iterator j = (*i)->actor->lines.begin();

			while(j != (*i)->actor->lines.end())
			{
				if (getDist(x, y, (*i)->x + (*i)->actor->center_x + (*j)->x1[(*i)->angle], (*i)->y + (*i)->actor->center_y + (*j)->y1[(*i)->angle], (*i)->x + (*i)->actor->center_x + (*j)->x2[(*i)->angle], (*i)->y + (*i)->actor->center_y + (*j)->y2[(*i)->angle]) < (size * size))
				{
					//return true;
					result.push_back(*i);
					cont = true;
					break;
				}
				j++;
			}

			if (cont)
			{
				i++;
				continue;
			}

			std::vector<circle*>::iterator k = (*i)->actor->circles.begin();

			while (k != (*i)->actor->circles.end())
			{
				if (getDist((*i)->x + (*i)->actor->center_x + (*k)->x[(*i)->angle] - x, (*i)->y + (*i)->actor->center_y + (*k)->y[(*i)->angle] - y) < ((*i)->getActor()->collision + size) * ((*k)->r + size))
				{
					//return true;
					result.push_back(*i);
					break;
				}
				k++;
			}

			i++;
		}

		return result;
	}
}